/**
 * 
 */
package ch.unizh.ifi.ailab.ai;

import org.apache.log4j.Logger;

import ch.unizh.ifi.ailab.config.ProgramSettings;
import ch.unizh.ifi.ailab.dataTypes.Command;
import ch.unizh.ifi.ailab.dataTypes.SensorData;
import ch.unizh.ifi.ailab.dataTypes.Vector2D;


/**
 * @author hans jaeckle jaeckle@gmail.com
 * 
 */
public class SMCVector extends SensorMotorCoupling {

	// properties
	private static ProgramSettings myProp = ProgramSettings
			.getProgramSettings();

	// maximas:
	private double maxNorm = 2 * 255 * (Math.cos(myProp
			.getIntProperty("sensor1Angle")) + Math.cos(myProp
			.getIntProperty("sensor2Angle")));

	private double maxAngle = 35; // in degrees!

	// speed normalization (set to 7 to not get maxspeed of 9)
	private double normSpeed = 7 / maxNorm;

	// direction normalization polar:
	private double normAngle = 10 / maxAngle; // normed to degrees!

	// sicherheitsabstand 60 = ca 50cm
	private int securityDist = 15;

	// logging
	private static Logger myLog = Logger.getLogger(AICore.class);

	public SMCVector() {
		myLog.debug("smc: initialisiere vektor smc.");
	}

	public void calculateMotionCommand(SensorData data, Command command)
			throws UnavoidableObjectException {
		Vector2D s1 = new Vector2D(data.getIo1(), Math.toRadians(myProp
				.getIntProperty("sensor1Angle")));
		Vector2D s2 = new Vector2D(data.getIo2(), Math.toRadians(myProp
				.getIntProperty("sensor2Angle")));
		Vector2D s3 = new Vector2D(data.getIo3(), Math.toRadians(myProp
				.getIntProperty("sensor3Angle")));
		Vector2D s4 = new Vector2D(data.getIo4(), Math.toRadians(myProp
				.getIntProperty("sensor4Angle")));

		// new vector
		Vector2D resultingVec = new Vector2D(s1);
		resultingVec.plus(s2);
		resultingVec.plus(s3);
		resultingVec.plus(s4);

		// normalized vectors:
		double newSpeed = resultingVec.getNorm() * normSpeed
				+ myProp.getIntProperty("speedStop");
		// (-1) case direction has changed after building in the new speed
		// controller...
		double newDir = (resultingVec.getAngle() * Math.toRadians(normAngle))
				* (-1) + myProp.getIntProperty("dirCenter");

		// notstop oder kommando setzen:
		if (data.getIo1() < securityDist || data.getIo2() < securityDist
				|| data.getIo3() < securityDist || data.getIo4() < securityDist) {
			throw new UnavoidableObjectException("ki: panik; werte: 1:"
					+ data.getIo1() + " 2:" + data.getIo2() + " 3:"
					+ data.getIo3() + " 4:" + data.getIo4()
					+ " security distance: " + securityDist);
		} else {
			// set command
			command.setSpeed((int) Math.round(newSpeed));
			command.setDirection((int) Math.round(newDir));
		}
		myLog.warn("smc: neues kommando: " + command.toString());
	}
}
